package ar.edu.untref.robotica.simulador;

import ar.edu.untref.robotica.motorfisico.Calculadora;
import ar.edu.untref.robotica.motorfisico.Rueda;
import ar.edu.untref.robotica.motorfisico.Sensor;

public class Robot {

	private Sensor sensorFront1;
	private Sensor sensorFront2;
	private Sensor sensor45Derecha;
	private Sensor sensor45Izquierda;
	private Sensor sensorAtras;
	private Rueda ruedaI;
	private Rueda ruedaD;
	// En centimetros
	private double alto;
	private double ancho;

	public Robot(double alto, double ancho) {

		Configuracion configuracion = Configuracion.instancia();
		this.sensorFront1 = new Sensor(configuracion.getSensorFrontal1Angulo(),
				configuracion.getSensorFrontal1X(),
				configuracion.getSensorFrontal1Y());
		this.sensorFront2 = new Sensor(configuracion.getSensorFrontal2Angulo(),
				configuracion.getSensorFrontal2X(),
				configuracion.getSensorFrontal2Y());
		this.sensor45Derecha = new Sensor(
				configuracion.getSensorDerechoAngulo(),
				configuracion.getSensorDerechoX(),
				configuracion.getSensorDerechoY());
		this.sensor45Izquierda = new Sensor(
				configuracion.getSensorIzquierdoAngulo(),
				configuracion.getSensorIzquierdoX(),
				configuracion.getSensorIzquierdoY());
		this.sensorAtras = new Sensor(configuracion.getSensorAtrasAngulo(),
				configuracion.getSensorAtrasX(),
				configuracion.getSensorAtrasY());
		this.alto = alto;
		this.ancho = ancho;
		this.ruedaI = new Rueda();
		this.ruedaD = new Rueda();
		// HARDCODE
		this.ruedaI.setDiametroRueda(5);
		this.ruedaD.setDiametroRueda(5);
	}

	public Rueda getRuedaI() {

		return this.ruedaI;
	}

	public Rueda getRuedaD() {

		return this.ruedaD;
	}

	public double getAlto() {

		// nunca es usado
		return this.alto;
	}

	public double getAncho() {

		// nunca es usado
		return this.ancho;
	}

	public double getDistanciaObstaculoAdelante(Escenario escenario) {

		return (this.sensorFront1.sensar(escenario) + this.sensorFront2
				.sensar(escenario)) / 2;
	}

	public double getDistanciaObstaculoAtras(Escenario escenario) {

		return this.sensorAtras.sensar(escenario);
	}

	public double getDistanciaObstaculo45Derecha(Escenario escenario) {

		return this.sensor45Derecha.sensar(escenario);
	}

	public double getDistanciaObstaculo45Izquierda(Escenario escenario) {

		return this.sensor45Izquierda.sensar(escenario);
	}

	public double getVelocidadDesplazamiento() {

		return Calculadora.getVelocidadDesplazamiento(getRuedaI()
				.getVelocidad().doubleValue(), getRuedaD().getVelocidad().doubleValue());
	}
}
